#include "ArmorSelector.hpp"

#include "../Modules/GeometryFeatureModule.hpp"

#include <tbb/tbb.h>
#include <cmath>

#ifdef DEBUG
#include <iostream>
#endif

namespace RoboPioneers::Prometheus::Core
{
	using RotatedRectPair = std::tuple<cv::RotatedRect, cv::RotatedRect>;

	/// 计算装甲板得分
	double ArmorSelector::GetArmorScore(const std::tuple<cv::RotatedRect, cv::RotatedRect>& armor) const
	{
		auto first_light = Modules::GeometryFeatureModule::StandardizeRotatedRectangle(std::get<0>(armor));
		auto second_light = Modules::GeometryFeatureModule::StandardizeRotatedRectangle(std::get<1>(armor));

		auto length = cv::norm(first_light.Center - second_light.Center);
		auto width = first_light.Length > second_light.Length ? first_light.Length : second_light.Length;

		auto center_point = (first_light.Center + second_light.Center) / 2;
		auto real_center_point = center_point + *PositionOffset;
		auto real_offset = cv::norm(real_center_point - (cv::Point(ScreenWidth, ScreenHeight) / 2));

        auto score =  - static_cast<long> (real_offset);
		return score;
	}

	/// 粗略估计距离
    double_t ArmorSelector::GetEstimatedDistance(std::vector<cv::Point2d> corner_points) const
	{
        cv::Mat rVec = cv::Mat::zeros(3, 1, CV_64FC1);//init rvec
        cv::Mat tVec = cv::Mat::zeros(3, 1, CV_64FC1);//init tvec
        solvePnP(Obj, corner_points, CameraMatrix,
                 DistCoeff, rVec, tVec, false, cv::SOLVEPNP_ITERATIVE);
        std::cout<<tVec<<std::endl;
        return tVec.at<double>(2);
	}

	void ArmorSelector::Execute()
	{
		if (Armors->empty())
		{
			// 直接设置未找到，不更新兴趣区，由外部程序自行决定
			Found = false;

			return;
		}
		else
		{
			using scored_pair = std::tuple<double, RotatedRectPair>;

			// 更坏比较器
			struct worse_comparer
			{
				// 该操作符返回true，则a会排到b的后方
				bool operator () (const scored_pair& a, const scored_pair& b)
				{
					return std::get<0>(a) < std::get<0>(b);
				}
			};

			// 计算过分数的装甲板队列
			tbb::concurrent_priority_queue<scored_pair, worse_comparer> scored_armors;

			// 并行地计算分数并放入优先级队列
			tbb::parallel_for_each(*Armors, [this, &scored_armors](const RotatedRectPair& armor_candidate){
				scored_armors.push({this->GetArmorScore(armor_candidate), armor_candidate});
			});

			//==============================
			// 解包获取最优项
			//==============================

			scored_pair best_scored_pair;
			while(!scored_armors.try_pop(best_scored_pair));
			RotatedRectPair best_pair = std::get<1>(best_scored_pair);

			auto& first_light = std::get<0>(best_pair);
			auto& second_light = std::get<1>(best_pair);
			if(first_light.center.x > second_light.center.x) swap(first_light,second_light);

			auto center_point = (first_light.center + second_light.center) / 2;

			//==============================
			// 计算兴趣区
			//==============================

			std::vector<cv::Point2f> armor_vertices;
			armor_vertices.resize(8);
			first_light.points(&armor_vertices[0]);
			second_light.points(&armor_vertices[4]);
			auto armor_rectangle = cv::minAreaRect(armor_vertices).boundingRect();
			ArmorBox = armor_rectangle;
			ArmorBox.x += PositionOffset->x;
			ArmorBox.y += PositionOffset->y;

			auto& global_offset = *PositionOffset;

			//==============================
			// 填充输出结果
			//==============================

			// 计算并限制兴趣区长宽
			InterestedArea.width = armor_rectangle.width * (WidthExpandRatio + 1);
			if (InterestedArea.width < LockingBoxMinWidth) InterestedArea.width = LockingBoxMinWidth;
			InterestedArea.height = armor_rectangle.height * (HeightExpandRatio + 1);
			if (InterestedArea.height < LockingBoxMinHeight) InterestedArea.height = LockingBoxMinHeight;

			// 计算并限制兴趣区坐标
			InterestedArea.x = global_offset.x + armor_rectangle.x - 0.5f * (InterestedArea.width - armor_rectangle.width);
			if (InterestedArea.x < 0) InterestedArea.x = 0;
			InterestedArea.y = global_offset.y + armor_rectangle.y - 0.5f * (InterestedArea.height - armor_rectangle.height);
			if (InterestedArea.y < 0) InterestedArea.y = 0;

			// 结合兴趣区坐标限制兴趣区长宽
			if (InterestedArea.x + InterestedArea.width > ScreenWidth)
				InterestedArea.width = ScreenWidth - InterestedArea.x;
			if (InterestedArea.y + InterestedArea.height > ScreenHeight)
				InterestedArea.height = ScreenHeight - InterestedArea.y;


			// 获取灯条的4个角点 左上 左下 右下 右上
			std::vector<cv::Point2d> corner_points;
			auto first_light_std = Modules::GeometryFeatureModule::StandardizeRotatedRectangle(first_light);
			auto second_light_std = Modules::GeometryFeatureModule::StandardizeRotatedRectangle(second_light);
//			std::cout<<first_light_std.Center.x<<' '<<first_light_std.Length<<' '<<first_light_std.Angle<<std::endl;
//			std::cout<<second_light_std.Center.x<<' '<<second_light_std.Length<<' '<<second_light_std.Angle<<std::endl;
			corner_points.push_back(cv::Point2d (first_light_std.Center.x + first_light_std.Length*cos(first_light_std.Angle*CV_PI/180.f)/2.f,
                                                 first_light_std.Center.y - first_light_std.Length * sin(first_light_std.Angle*CV_PI/180.f)/2.f));
			corner_points.push_back(cv::Point2d (first_light_std.Center.x - first_light_std.Length* cos(first_light_std.Angle*CV_PI/180.f)/2.f,
                                                first_light_std.Center.y + first_light_std.Length * sin(first_light_std.Angle*CV_PI/180.f)/2.f));
            corner_points.push_back(cv::Point2d (second_light_std.Center.x - second_light_std.Length* cos(second_light_std.Angle*CV_PI/180.f)/2.f,
                                                 second_light_std.Center.y + second_light_std.Length * sin(second_light_std.Angle*CV_PI/180.f)/2.f));;
			corner_points.push_back(cv::Point2d (second_light_std.Center.x + second_light_std.Length*cos(second_light_std.Angle*CV_PI/180.f)/2.f,
                                                 second_light_std.Center.y - second_light_std.Length * sin(second_light_std.Angle*CV_PI/180.f)/2.f));
            for(int i=0;i<4;i++)
            {
                std::cout<<corner_points[i]<<' ';
            }
            std::cout<<std::endl;

			// 估算距离
			Distance = GetEstimatedDistance(corner_points);

			Found = true;

			// 装甲板中心坐标加上ROI的起始点坐标得到全局坐标
			X = static_cast<int>(center_point.x) + global_offset.x;
			Y = static_cast<int>(center_point.y) + global_offset.y;

			// 坐标更新为相对于屏幕中心的坐标
			X = X - ScreenWidth/2;
			Y = -(Y - ScreenHeight/2);
            // 存储当前计算坐标的时刻
			now = std::chrono::steady_clock::now();
			#ifdef DEBUG
//			std::cout << "Width:" << armor_rectangle.width << " Height:" << armor_rectangle.height << std::endl;
			#endif
		}
	}
}